
// import oimo.collision.geometry.*;
// import oimo.collision.narrowphase.*;
// import oimo.common.MathUtil;
// import oimo.common.Transform;
// import oimo.m.Vec3;
// import oimo.m.M;

import { Detector } from "./Detector";
import { DetectorResult } from "../DetectorResult";
import { Geometry } from "../../geometry/Geometry";
import { Transform } from "../../../common/Transform";
import { CachedDetectorData } from "./CachedDetectorData";
import { SphereGeometry } from "../../geometry/SphereGeometry";
import { Vec3 } from "../../../common/Vec3";
import { M } from "../../../common/M";
import { MathUtil } from "../../../common/MathUtil";
import { CapsuleGeometry } from "../../geometry/CapsuleGeometry";

/**
 * Sphere vs Capsule detector.
 */

export class SphereCapsuleDetector extends Detector {
	/**
	 * If `swapped` is `true`, the collision detector expects `CapsuleGeometry` and `SphereGeometry` for the
	 * first and second argument of `SphereCapsuleDetector.detect`. If `swapped` is `false`, the collision detector expects
	 * `SphereGeometry` and `CapsuleGeometry` instead.
	 */
	constructor(swapped: boolean) {
		super(swapped);
	}

	public detectImpl(result: DetectorResult, geom1: Geometry, geom2: Geometry, tf1: Transform, tf2: Transform, cachedData: CachedDetectorData): void {
		var s1: SphereGeometry = geom1 as SphereGeometry;
		var c2: CapsuleGeometry = geom2 as CapsuleGeometry;

		result.incremental = false;

		var hh2: number = c2._halfHeight;
		var r1: number = s1._radius;
		var r2: number = c2._radius;

		// capsule axis
		var axis2: Vec3 = new Vec3();
		M.mat3_getCol(axis2, tf2._rotation, 1);

		// closest point 1
		var cp1: Vec3 = new Vec3();
		M.vec3_assign(cp1, tf1._position);

		// find closest point on segment 2

		// line segment (p2, q2)
		var p2: Vec3 = new Vec3();
		var q2: Vec3 = new Vec3();
		M.vec3_addRhsScaledTo(p2, tf2._position, axis2, -hh2);
		M.vec3_addRhsScaledTo(q2, tf2._position, axis2, hh2);

		var p12: Vec3 = new Vec3();
		M.vec3_sub(p12, cp1, p2);

		// q - p
		var d2: Vec3 = new Vec3();
		M.vec3_sub(d2, q2, p2);

		var d22: number = hh2 * hh2 * 4;

		var t: number = M.vec3_dot(p12, d2);
		if (t < 0) t = 0;
		else if (t > d22) t = 1;
		else t /= d22;

		var cp2: Vec3 = new Vec3();
		M.vec3_addRhsScaledTo(cp2, p2, d2, t);

		// perform sphere vs sphere collision

		var d: Vec3 = new Vec3();
		M.vec3_sub(d, cp1, cp2);
		var len2: number = M.vec3_dot(d, d);
		if (len2 >= (r1 + r2) * (r1 + r2)) return;
		var len: number = MathUtil.sqrt(len2);
		var n: Vec3 = new Vec3();
		if (len > 0) {
			M.vec3_scale(n, d, 1 / len);
		} else {
			n.init(1, 0, 0);
		}
		this.setNormal(result, n);
		var pos1: Vec3 = new Vec3();
		var pos2: Vec3 = new Vec3();
		M.vec3_addRhsScaledTo(pos1, cp1, n, -r1);
		M.vec3_addRhsScaledTo(pos2, cp2, n, r2);
		this.addPoint(result, pos1, pos2, r1 + r2 - len, 0);
	}
}
